#include "HandEyeCalibration.h"

int main()
{

    std::vector<cv::Mat> R_gripper2base;
    std::vector<cv::Mat> t_gripper2base;
    std::vector<cv::Mat> R_target2cam;
    std::vector<cv::Mat> t_target2cam;
    cv::Mat R_cam2gripper; // 3*3
    cv::Mat t_cam2gripper; // 3*1

    for (int i = 0; i < 5; i++)
    {
        // 欧拉角转旋转矩阵
        cv::Mat eulerAngle(1, 3, CV_64F);
        cv::Mat R_g2b = eulerAngleToRotatedMatrix(eulerAngle, "xyz");
        R_gripper2base.push_back(cv::Mat::eye(3, 3, CV_64F));
    }

    cv::calibrateHandEye(
        R_gripper2base,
        t_gripper2base,
        R_target2cam,
        t_target2cam,
        R_cam2gripper,
        t_cam2gripper,
        CALIB_HAND_EYE_TSAI
    );
    return 0;
}